package LeetDev.a3d;

public class Quat {
	public static final Quat identity=new Quat();
	public float w;
	public float x;
	public float y;
	public float z;

	public Quat()
	{
		w=1;x=y=z=0;
	}
	public Quat(final float n)
	{
		w=n;x=y=z=0;
	}
	public Quat(final Quat q)
	{
		w=q.w;
		x=q.x;
		y=q.y;
		z=q.z;
	}
	public Quat(final float wn, final float xn, final float yn, final float zn)
	{
		w=wn;
		x=xn;
		y=yn;
		z=zn;
	}

	public void set(final float xa, final float ya, final float za)
	{
		float cr, cp, cy, sr, sp, sy, cpcy, spsy;
		  
		cr=(float)Math.cos(xa/2.0f);
		cp=(float)Math.cos(ya/2.0f);
		cy=(float)Math.cos(za/2.0f);
	  
		sr=(float)Math.sin(xa/2.0f);
		sp=(float)Math.sin(ya/2.0f);
		sy=(float)Math.sin(za/2.0f);
	  
		cpcy=cp*cy;
		spsy=sp*sy;
	  
		w=cr*cpcy+sr*spsy;
		x=sr*cpcy-cr*spsy;
		y=cr*sp*cy+sr*cp*sy;
		z=cr*cp*sy-sr*sp*cy;
	}

	public void set(final Matrix ma)
	{
		float tr, s;
		tr=ma.m[0]+ma.m[5]+ma.m[10]+1.0f;
		if(Math.abs(tr)>0.0001f)
		{
			s=0.5f/(float)Math.sqrt(tr);
			w=0.25f/s;
			x=(ma.m[6]-ma.m[9])*s;
			y=(ma.m[8]-ma.m[2])*s;
			z=(ma.m[1]-ma.m[4])*s;
		}
		else
		{
			if(ma.m[5]<ma.m[0]&&ma.m[10]<ma.m[0])
			{
				s=0.5f/(float)Math.sqrt(1.0f+ma.m[0]-ma.m[5]-ma.m[10]);
				x=0.25f/s;
				y=(ma.m[4]+ma.m[1])*s;
				z=(ma.m[8]+ma.m[2])*s;
				w=(ma.m[6]-ma.m[9])*s;
			}
			else if(ma.m[10]<ma.m[5])
			{
				s=0.5f/(float)Math.sqrt(1.0f+ma.m[5]-ma.m[10]-ma.m[0]);
				x=(ma.m[1]+ma.m[4])*s;
				y=0.25f/s;
				z=(ma.m[9]+ma.m[6])*s;
				w=(ma.m[8]-ma.m[2])*s;
			}
			else
			{
				s=0.5f/(float)Math.sqrt(1.0f+ma.m[10]-ma.m[0]-ma.m[5]);
				x=(ma.m[2]+ma.m[8])*s;
				y=(ma.m[6]+ma.m[9])*s;
				z=0.25f/s;
				w=(ma.m[1]-ma.m[4])*s;
			}
		}
	}

	public float dot(final Quat b)
	{
		return(w*b.w+x*b.x+y*b.y+z*b.z);
	}
	
	public void neg(Quat result)
	{
		result.w=-w;
		result.x=-x;
		result.y=-y;
		result.z=-z;
	}
	
	public void unitNeg(Quat result)
	{
		result.w=w;
		result.x=-x;
		result.y=-y;
		result.z=-z;
	}
	
	public void exp(Quat result)
	{
		float angle,sn,c;
		angle=(float)Math.sqrt(x*x+y*y+z*z);
		result.w=(float)Math.cos(angle);
		sn=(float)Math.sin(angle);
		if(Math.abs(sn)>=0.0001f)
		{
			c=sn/angle;
			result.x=c*x;
			result.y=c*y;
			result.z=c*z;
		}
		else
		{
			result.x=x;
			result.y=y;
			result.z=z;
		}
	}
	
	public void log(Quat result)
	{
		float angle,sn,c;
		result.w=0;
		angle=(float)Math.acos(w);
		sn=(float)Math.sin(angle);
		if(Math.abs(w)<1.0f && Math.abs(sn)>=0.0001f)
		{
			c=angle/sn;
			result.x=c*x;
			result.y=c*y;
			result.z=c*z;
		}
		else
		{
			result.x=x;
			result.y=y;
			result.z=z;
		}
	}
	
	public void inverse(Quat result)
	{
		float n=w*w+x*x+y*y+z*z;
		if(n!=0.0f)
		{
			n=1/n;
			result.w=w*n;
			result.x=-x*n;
			result.y=-y*n;
			result.z=-z*n;
		}
		else
		{
			result.w=1;
			result.x=0;
			result.y=0;
			result.z=0;
		}
	}
	
	public void normalize(Quat result)
	{
		float n=w*w+x*x+y*y+z*z;
		if(n!=0.0f)
		{
			n=(float)(1/Math.sqrt(n));
			result.w=w*n;
			result.x=x*n;
			result.y=y*n;
			result.z=z*n;
		}
		else
		{
			result.w=1;
			result.x=0;
			result.y=0;
			result.z=0;
		}
	}
	
	public void mul(final Quat q, Quat result)  
	{
		result.w=w*q.w-x*q.x-y*q.y-z*q.z;
		result.x=w*q.x+x*q.w+y*q.z-z*q.y;
		result.y=w*q.y+y*q.w+z*q.x-x*q.z;
		result.z=w*q.z+z*q.w+x*q.y-y*q.x;
	}

	public void mul(final float n, Quat result)
	{
		result.w=w*n;
		result.x=x*n;
		result.y=y*n;
		result.z=z*n;
	}

	public void div(final float n, Quat result)
	{
		result.w=w/n;
		result.x=x/n;
		result.y=y/n;
		result.z=z/n;
	}

	public void add(final Quat q, Quat result)
	{
		result.w=w+q.w;
		result.x=x+q.x;
		result.y=y+q.y;
		result.z=z+q.z;
	}
	
	public void sub(final Quat q, Quat result)  
	{
		result.w=w-q.w;
		result.x=x-q.x;
		result.y=y-q.y;
		result.z=z-q.z;
	}
}
